#ifndef _PLANNER_MANAGER_H_
#define _PLANNER_MANAGER_H_

#include <stdlib.h>

#include <bspline_opt/bspline_optimizer.h>
#include <traj_utils/uniform_bspline.h>
#include <plan_env/grid_map.h>
#include <traj_utils/plan_container.hpp>
#include <ros/ros.h>
#include <traj_utils/planning_visualization.h>
#include <bezier_predict/predictor.h>
#include <plan_manage/tracking_astar.hpp>
#include <plan_manage/utils_algorithm.hpp>
#include <plan_manage/colors.h>

namespace ego_planner {
// Key algorithms of mapping and planning are called
class PlanManager
{
public:
  PlanManager();
  ~PlanManager();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /* main planning interface */
  bool reboundReplan(Eigen::Vector3d start_p, Eigen::Vector3d start_v,
                     Eigen::Vector3d start_a,
                     Eigen::Vector3d end_p, Eigen::Vector3d end_v,
                     bool flag_polyInit, bool flag_randomPolyTraj);
  bool reboundReplanForPredict(Eigen::MatrixXd &ctrl_pts, double ts);
  bool kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
                         Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
                         Eigen::Vector3d end_vel);
  bool EmergencyStop(Eigen::Vector3d stop_pos);
  bool planGlobalTraj(Eigen::Vector3d start_pos, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
                      Eigen::Vector3d end_pos, Eigen::Vector3d end_vel, Eigen::Vector3d end_acc);

  void initPlanModules(ros::NodeHandle &nh, Visualization::Ptr vis = NULL);

  bool checkCollision(int drone_id);
  void angleLimite(double &angle);

  void updateTrajEntropyMap();
  void updateDynamicEntropyMap();
  bool planYaw(Eigen::Vector3d pos, Eigen::Quaterniond quad);
  bool planYawwithTraj(Eigen::Quaterniond quad);
  bool getEntropyatTimeT(double t, Eigen::Vector3d &pos, Eigen::Vector3d &vel,
                         double &yaw_uav,
                         std::vector<double> &angel_vec,
                         std::vector<double> &entropy_vec,
                         bool isLimit = false, double t_last = 0,
                         double yaw_last = 0);
  bool findKeyViewPoint(Eigen::Vector3d pos, double t_cur, double &t_key);
  void filterAngle(std::vector<double> &angle_vec, std::vector<double> &entropy_vec,
                   double t, double t_last, double yaw_last);

  bool findMaxIndex(const std::vector<double> &data, int length,
                    int num, std::vector<int> &ret);
  std::vector<double> getUniformSamples(double start, double end, int n);

  PlanParameters pp_;
  PlanYawParameters py_;
  LocalTrajData local_data_;
  GlobalTrajData global_data_;
  YawData yaw_data_;
  EntropyMap::Ptr grid_map_;
  SwarmTrajData swarm_trajs_buf_;
  ObsPosesPtr obs_poses_{nullptr};
  ObsTrajsPtr obs_trajs_{nullptr};
  BsplineOptimizer::Ptr bspline_optimizer_;
  Predictor::Ptr predictor_;
  unique_ptr<KinodynamicAstar> kino_path_finder_;
  TopFinder topfinder_;
  GimbalOptimizer gimOpter_;

  double yaw_target_;
  bool detect_obs_{false};
  bool have_reset_dynamic_map_{true};

private:
  int replan_cnt_ = 0;
  int continous_failures_count_ = 0;
  bool flag_first_call_ = true;
  bool flag_force_polynomial_ = false;

  /* main planning algorithms & modules */
  Visualization::Ptr visualization_;

  void updateYawTrajInfo(const UniformBspline &yaw_traj, const ros::Time time_now);

  void updateTrajInfo(const UniformBspline &position_traj);

  void reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &terminal_constraint, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt);

  bool refineTraj(UniformBspline &traj, vector<Eigen::Vector3d> &terminal_constraint, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);

public:
  typedef unique_ptr<PlanManager> Ptr;
};
} // namespace ego_planner

#endif